#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/frames.hpp"

#include <iostream>

Eigen::Matrix<double,3,3> eul2Rot(double roll, double pitch, double yaw); // 欧拉角转旋转矩阵，ZYX顺序
Eigen::Vector3d Rot2eul(const Eigen::Matrix3d &rot);
Eigen::MatrixXd pseudoInv_SVD(const Eigen::MatrixXd &mat);
double adjustNumber(double num, double lowerBound, double upperBound);

int main(int argc, char ** argv)
{
	// 导入urdf模型，初始化数据容器
	const std::string urdf_robot_filename = std::string("../model/AzureLoong/AzureLoong.urdf");
	pinocchio::Model model_biped;
	pinocchio::JointModelFreeFlyer root_joint;
	pinocchio::urdf::buildModel(urdf_robot_filename, root_joint, model_biped);

	// display all the joints' ids and names
	// 注意，这个库会在头上加个universe的joint，导致这里显示的joint个数比urdf里面的多一个
	// 但是在存放关节位置中的q向量中，加的这个关节是不算个数的，因此用此处显示的jointid去q中读取数据时要减1
	for(pinocchio::JointIndex jointIdx = 0; jointIdx < (pinocchio::JointIndex)model_biped.njoints; jointIdx++)
		std::cout << std::setw(10) << std::left
				  << jointIdx << ' ' << model_biped.names[jointIdx] << std::endl;

	// get joint id according to the joint name
	auto l_ankle_Joint=model_biped.getJointId("left-ankle-pitch");
	auto r_ankle_Joint=model_biped.getJointId("right-ankle-pitch");

	// 增加足底坐标系
	std::string FE_Frame_NameStr[]={{"FE_Fra_left_1"},{"FE_Fra_left_2"},{"FE_Fra_left_3"},{"FE_Fra_left_4"},
									{"FE_Fra_right_1"},{"FE_Fra_right_2"},{"FE_Fra_right_3"},{"FE_Fra_right_4"}};
	double xSign[]={1,1,-1,-1,1,1,-1,-1};
	double ySign[]={1,-1,-1,1,1,-1,-1,1};

	pinocchio::SE3 FE_Off;
	Eigen::Matrix3d R; // 新增足底坐标系的偏置旋转矩阵
	Eigen::Vector3d p; // 新增足底坐标系的偏置位移向量
	R.setIdentity();
	FE_Off.rotation_impl(R);
	for (int i=0;i<8;i++)
	{
		p<<0.03+xSign[i]*0.17/2.,ySign[i]*0.015,-0.024;
		FE_Off.translation_impl(p);
		pinocchio::JointIndex parentJoint;
		if (i<4)
			parentJoint=l_ankle_Joint;
		else
			parentJoint=r_ankle_Joint;

		pinocchio::Frame footEnd(FE_Frame_NameStr[i],parentJoint,0,FE_Off,pinocchio::FIXED_JOINT,pinocchio::Inertia::Zero());
		model_biped.addFrame(footEnd); // 在已导入模型中增加坐标系
	}

	pinocchio::FrameIndex FE_end_Idx[8];
	std::cout << "FE_end_Idx: " << std::endl;
	for (int i=0;i<8;i++){
		FE_end_Idx[i]=model_biped.getFrameId(FE_Frame_NameStr[i]);
		std::cout << "[" << i <<"]" << FE_Frame_NameStr[i] << ": "<< FE_end_Idx[i] << std::endl;
	}

	pinocchio::Data data_biped(model_biped);

	//================================================================================
	// 设置q及dq的值
	// q=[global_base_position, global_base_quaternion, joint_positions], 其中关节顺序是从前述关节序号为2的关节开始
	// quaternion=[x,y,z,w]
	// dq=[local_base_velocity_linear, local_base_velocity_angular, joint_velocities]
//        Eigen::VectorXd q = pinocchio::neutral(model_biped);
	Eigen::VectorXd q = pinocchio::randomConfiguration(model_biped);
	q(0)=0.0;q(1)=0;q(2)=0; // random config 会使浮动基位置变为inf，需要手动清零

	q.setZero();
	q.block(0,0,12,1) = Eigen::Vector<double, 12>(0.0,0.0,-0.0214,0.2080,-0.1866,0.0,
										0.0,0.0,-0.0214,0.2080,-0.1866,0.0);
	Eigen::Matrix<double, 3, 3> Rcur = eul2Rot(0., 0, 0.0*3.1415926);
	Eigen::Quaternion<double> quatCur;
	quatCur = Rcur; //旋转矩阵转四元素
	q(3) = quatCur.x();
	q(4) = quatCur.y();
	q(5) = quatCur.z();
	q(6) = quatCur.w();

	Eigen::VectorXd dq = Eigen::VectorXd::Ones(model_biped.nv) * 0.00;
	dq(0)=0.0;dq(1)=0;dq(2)=0;
	dq(3)=0.0;dq(4)=0;dq(5)=0;

	std::cout << std::fixed << std::setprecision(4);
	// 计算M矩阵
	pinocchio::crba(model_biped, data_biped, q);
	// 由于M为对称矩阵，原始data中只存储了矩阵上三角的数据，此处需要恢复
//	std::cout << "M = " << std::endl << data_biped.M << std::endl;
	data_biped.M.triangularView<Eigen::Lower>() = data_biped.M.transpose().triangularView<Eigen::Lower>();
//	std::cout << "M = " << std::endl << data_biped.M << std::endl;

	// 计算C矩阵
	pinocchio::computeCoriolisMatrix(model_biped, data_biped, q, dq);
//	std::cout << "C=" << std::endl << data_biped.C << std::endl;

	// 计算G向量
	pinocchio::computeGeneralizedGravity(model_biped, data_biped, q);
//	std::cout << "G=" << std::endl << data_biped.g.transpose() << std::endl;

	// 计算全身等效转动惯量及质心位置，相对于pelvis关节的本体坐标系
	pinocchio::ccrba(model_biped, data_biped, q, dq);
	std::cout << "centroid inertia" << std::endl << data_biped.Ig << std::endl;
	std::cout << "COM " << std::endl << data_biped.com[0].transpose() << std::endl;

	// 计算关节的雅科比矩阵，相对于pelvis，在本体坐标系下。该矩阵前三行对应末端线速度，后三行对应末端角速度
	Eigen::MatrixXd J_r(6, model_biped.nv);
	Eigen::MatrixXd J_l(6, model_biped.nv);
	J_r.setZero(); // Eigen 创建矩阵后需要初始化，否则后续结果会有问题，可能个别元素会出现nan
	J_l.setZero();
	auto *J_FE = new Eigen::MatrixXd[8];
	for (int i = 0; i < 8; i++)
		J_FE[i] = Eigen::MatrixXd(6, model_biped.nv);
	pinocchio::forwardKinematics(model_biped, data_biped, q); // 计算雅科比之前必须调用正运动学
	std::cout << "ankle.pos=" << data_biped.oMi[l_ankle_Joint].translation().transpose() << std::endl;
	std::cout << "ankle.rot=" << data_biped.oMi[l_ankle_Joint].rotation().transpose() << std::endl;
	pinocchio::updateFramePlacements(model_biped, data_biped); //更新frame
	auto l_ankle_frame = model_biped.getFrameId("J_ankle_l_roll");
	pinocchio::computeJointJacobians(model_biped, data_biped, q); // 计算所有关节的雅科比矩阵
	pinocchio::getJointJacobian(model_biped, data_biped, r_ankle_Joint, pinocchio::LOCAL,
								J_r); // 提取指定关节的雅科比矩阵，在本体坐标系下
	pinocchio::getJointJacobian(model_biped, data_biped, r_ankle_Joint, pinocchio::LOCAL,
								J_l); // 提取指定关节的雅科比矩阵，在本体坐标系下

	// 提取坐标系的雅科比矩阵，前置必须调用过forwardKinematics，updateFramePlacements，computeJointJacobians
	for (int i = 0; i < 8; i++) {
		pinocchio::getFrameJacobian(model_biped, data_biped, FE_end_Idx[i], pinocchio::LOCAL, J_FE[i]); // 提取指定坐标系的雅科比矩阵
	}
}

// 欧拉角转旋转矩阵，ZYX顺序
Eigen::Matrix<double, 3, 3> eul2Rot(double roll, double pitch, double yaw) {
	Eigen::Matrix<double,3,3> Rx,Ry,Rz;
	Rz<<cos(yaw),-sin(yaw),0,
			sin(yaw),cos(yaw),0,
			0,0,1;
	Ry<<cos(pitch),0,sin(pitch),
			0,1,0,
			-sin(pitch),0,cos(pitch);
	Rx<<1,0,0,
			0,cos(roll),-sin(roll),
			0,sin(roll),cos(roll);
	return Rz*Ry*Rx;
}
Eigen::Vector3d Rot2eul(const Eigen::Matrix3d &rot)
{
	Eigen::Vector3d eul;
	eul(0) = std::atan2(rot(2, 1), rot(2, 2));
	eul(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2)));
	eul(2) = std::atan2(rot(1, 0), rot(0, 0));
	return eul;
}

// 规整输入到上下界限之内，超出则通过加减2*3.1415进行调整
double adjustNumber(double num, double lowerBound, double upperBound) {
	if (num >= lowerBound && num <= upperBound) {
		return num;  // 已经在范围内，直接返回
	}

	// 如果数字小于a
	int k=0;
	if (num < lowerBound) {
		k = int(fabs(num-lowerBound)/(2*3.1415))+1;
		double res = num + k*2*3.1415;
		if (res <= upperBound) {  // 判断是否可以精确调整
			return res;  // 返回调整后的值
		}
		else
			return num;
	}
		// 如果数字大于b
	else if (num > upperBound) {
		k = int(fabs(num-upperBound)/(2*3.1415))+1;
		double res = num - k*2*3.1415;
		if (res >= lowerBound) {  // 判断是否可以精确调整
			return res;  // 返回调整后的值
		}
		else
			return num;
	}
	return num;
}